/**
  * @file       dsensor.c
  * @brief      距离传感器组件
  * @copyright  Copyright (c) 2020~2030 ShenZhen Dxtc Tech. Co., Ltd.
  * All rights reserved.
  * @version    V1.0
  * @author     TianXubin
  * @date       2021-10-22 17:53:29
  * @note       鼎新同创·智能锁
  *
  *****************************************************************************/

#include "device.h"
#include "component.h"
#include "tmf8801_driver.h"
#include "tmf8801_bin.h"
#include "nd03b.h"

// #define DSENSOR_DEBUG

#define DSENSOR_LOG(format, ...) OSAL_LOG(C_BLUE format C_NONE, ##__VA_ARGS__)
#define __DSENSOR_LOG(format, ...) __OSAL_LOG(C_BLUE format C_NONE, ##__VA_ARGS__)
#if defined(DSENSOR_DEBUG)
#define DSENSOR_DEBUG_LOG(format, ...) OSAL_LOG(C_BLUE format C_NONE, ##__VA_ARGS__)
#define __DSENSOR_DEBUG_LOG(format, ...) __OSAL_LOG(format, ##__VA_ARGS__)
#else
#define DSENSOR_DEBUG_LOG(format, ...)
#define __DSENSOR_DEBUG_LOG(format, ...)
#endif

#define VHW_PIN_AMS_POWER vPIN_C15
#define VHW_PIN_AMS_EN vPIN_C16
#define VHW_IRQ_AMS vPIN_I12

#define MAX_WAKE_DISTANCE 1000
#define VALID_THRESHOLD 50

#define EVENT_READ_DISTANCE (0X00000001)

typedef enum
{
	DEVICE_POWER_OFF,
	DEVICE_NO_FIRMWARE,
	DEVICE_READY,
	DEVICE_ERROR,
} Dsensor_Status_enum_t;

typedef enum
{
	TOF_POWEON,
	TOF_SLEEP,
	TOF_READ_DISTANCE,
	TOF_SETTINGS,
	TOF_CAL_RESULT,
} TofType_enum_t;

/* 传感器类型 */
typedef enum
{
	DSENSOR_UNKNOWN,
	DSENSOR_AMS,
	DSENSOR_ND,
} SensorType_enum_t;

typedef struct
{
	Dsensor_Sensitivity_enum_t cfi;
	uint8_t cal_data[14];
} Dsensor_Cfg_stu_t;

static FlagStatus tofCalFlag = RESET;
static struct tmf8801_ctx tmf8801;
__NVRAM static Dsensor_Status_enum_t device_status = DEVICE_POWER_OFF;
// __NVRAM static uint16_t baseline_value = MAX_WAKE_DISTANCE + VALID_THRESHOLD;
__EFRAM static FunctionalState sensor_swi = DISABLE;
__EFRAM static Dsensor_Sensitivity_enum_t sensor_sensitivity = DSENSOR_SENS_H;
__EFRAM static uint16_t measure_interval = 100;
__EFRAM static uint8_t demo_mode = 0;
__EFRAM static uint8_t cal_data[14] = {0xB1, 0x44, 0x03, 0xE6, 0xD4, 0x7F, 0x84, 0x03, 0x02, 0x03, 0x18, 0x2C, 0x0, 0x04};
const static uint8_t default_cal_data[14] = {0xB1, 0x44, 0x03, 0xE6, 0xD4, 0x7F, 0x84, 0x03, 0x02, 0x03, 0x18, 0x2C, 0x0, 0x04};

__NVRAM static DsensorMsg_t dsensor_msg;
__NVRAM static FlagStatus dsensor_wake_flag = RESET;
__NVRAM static uint8_t dsensor_err_cnt = 0;
__EFRAM static uint8_t err_flag = 0xff;
__EFRAM static SensorType_enum_t sensor_type = DSENSOR_UNKNOWN;

static void dsensor_irqHandle(char *name, uint8_t status, uint8_t times);

/**
  * @brief  故障处理
  * @note
  * @param  err：是否故障
  * @return void
  */
static void err_process(uint8_t err)
{
    if (err != err_flag)
    {
        err_flag = err;
        OSAL_MessagePublishErrorCode(ERRCODE_TYPE_DSENSOR, err_flag);
    }
}

__attribute__((weak)) int32_t ND03B_IsValid(void)
{
	return -1;
}

__attribute__((weak)) int32_t ND03B_SetWorkMode(uint8_t mode)
{
	return -1;
}

__attribute__((weak)) int32_t ND03B_Calibration_XTalk(void)
{
	return -1;
}

__attribute__((weak)) int32_t ND03B_Calibration_Offset(void)
{
	return -1;
}

__attribute__((weak)) int32_t ND03B_StartCapture(void)
{
	return -1;
}

__attribute__((weak)) int32_t ND03B_ReadDistance(uint16_t *ps, uint8_t *cfi)
{
	return -1;
}

__attribute__((weak)) void ND03B_DrvInit(void)
{

}

/**
  * @brief  判断传感器类型
  * @note   
  */
static SensorType_enum_t dsensor_find_sensor_type(void)
{
	if (sensor_type != DSENSOR_UNKNOWN)
	{
		return sensor_type;
	}

	/* 复位传感器 */
	Device_Enable(VHW_IIC_AMS);
	Device_Write(VHW_PIN_AMS_EN, 0, 0, 0);
	Device_DelayMs(1);
	Device_Write(VHW_PIN_AMS_POWER, 0, 0, 0); //打开电源
	Device_Write(VHW_PIN_AMS_EN, 0, 0, 1);
	Device_DelayMs(2);

	uint8_t close_sensor = 1;
	if (AMS_IsValid() == 0) //判断是否是ams
	{
		close_sensor = 0;
		sensor_type = DSENSOR_AMS;
	}
	else if (ND03B_IsValid() == 0) //判断是否是nd
	{
		close_sensor = 1;
		sensor_type = DSENSOR_ND;
	}
	__DSENSOR_LOG("dsensor_type: %d\r\n", sensor_type);

	if (sensor_swi == DISABLE || sensor_sensitivity == DSENSOR_SENS_OFF || close_sensor)
	{
		Device_Write(VHW_PIN_AMS_EN, 0, 0, 0);
	}
	return sensor_type;
}

/**
  * @brief  传感器初始化
  * @note   
  */
static void nd03b_init(void)
{
	static uint8_t init_flag = 0;
	/* 注册中断回调 */
	if (init_flag == 0)
	{
		init_flag = 1;

		InputPort_stu_t port[] = 
		{
			{"DSENSOR_INT", VHW_IRQ_AMS, INPUT_PORT_LOGIC_LOW, INPUT_PORT_FUNC_SINGLE}, //中断脚
        };
        /* 注册端口 */
        InputPort_Registered(port, OSAL_LENGTH(port), dsensor_irqHandle);

		ND03B_DrvInit();
	}
}

/**
  * @brief  读距离
  * @note   
  *         
  * @return SUCCESS/ERROR
  */
static ErrorStatus nd03b_ps_read(uint16_t *ps, uint8_t *cfi)
{
    return (ND03B_ReadDistance(ps, cfi) == 0 ? SUCCESS : ERROR);
}

/**
  * @brief  启动捕获
  * @note         
  */
static void nd03b_start_capture(void)
{
	// if (dsensor_err_cnt)
	// {
	// 	DSENSOR_LOG("dsensor err\r\n");
	// }
	// dsensor_err_cnt = 1;
    int32_t ret = ND03B_StartCapture();
	if (ret == 0)
	{
		device_status = DEVICE_READY;
	}
	else
	{
		device_status = DEVICE_ERROR;
	}
}

static ErrorStatus nd03b_calibration(void)
{
    dsensor_msg.type = CAL_RESULT;
    dsensor_msg.cal_msg.result = 1;
    /* 通知应用层校准结果 */
    OSAL_MessagePublish(&dsensor_msg, sizeof(DsensorMsg_t));
	return SUCCESS;
}


/**
  * @brief  判断数据有效性
  * @note   判断数据是否全0/0xff, 是表示无效数据, 返回ERROR
  *
  * @param  无
  */
static ErrorStatus _isDataValid(uint8_t *pData, uint8_t len)
{
	for (uint8_t i = 0; i < len; i++)
	{
		if (pData[i] > 0 && pData[i] < 0xff)
		{
			return SUCCESS;
		}
	}
	return ERROR;
}

/**
  * @brief  firmware download
  * @note   
  * @param  *client: pointer to tmf8801_ctx (struct tmf8801_ctx *)
  * @param  *BL_app: pointer to BL application struct
  * @param  *data:   firmware data
  * @param  size:    firmware size
  * @return 
  */
int8_t do_fwdl(void *client, struct tof8801_BL_application *BL_app, uint8_t *data, int size)
{
	#define ONE_PACKET_LEN 128
	#define AMS8801_RAM_ADDR 0x20000000

    int32_t ret = 0;
    uint16_t remain_len = size;
    int32_t send_len = ONE_PACKET_LEN;
    uint8_t *data_pos = data;

    /* 1.set address */
    ret = tof8801_BL_addr_ram(client, BL_app, AMS8801_RAM_ADDR);
    if (ret != 0)
    {
        return ret;
    }

    /* 2.loop write data */
	while (remain_len)
    {
		Device_FeedDog();
        send_len = (remain_len > ONE_PACKET_LEN) ? ONE_PACKET_LEN : remain_len;
        ret = tof8801_BL_write_ram(client, BL_app, data_pos, send_len);
        if (ret != 0)
        {
            return ret;
        }
        data_pos += send_len;
        remain_len -= send_len;
    }

    /* 3.perform remap & reset of tof */
    ret = tof8801_BL_ram_remap(client, BL_app);

	return ret;
}

static ErrorStatus tmf8801_firmware_download(void)
{
	int32_t error = 0;
	__DSENSOR_LOG("tof dl firmware");
	tof8801_BL_init_app(&tmf8801.BL_app);
	error = tof8801_BL_upload_init(&tmf8801, &tmf8801.BL_app, BL_DEFAULT_SALT);
	if (error)
	{
		return ERROR;
	}
	return (do_fwdl(&tmf8801, &tmf8801.BL_app, (char *)ams8801_bin_data, sizeof(ams8801_bin_data)) == 0 ? SUCCESS : ERROR);
}

static Dsensor_Status_enum_t tmf8801_checkWorkState(void)
{
	int32_t ret = 0;
	uint8_t result = 0;
	Dsensor_Status_enum_t status;
	ret = tof8801_app0_check_work_status(&tmf8801, &result);
	DSENSOR_DEBUG_LOG("read_reg 0x00 result %x, ret %x", result, ret);
	if (ret == 0)
	{
		if (result == 0x80) //AMS已复位，需重新下载固件
		{
			status = DEVICE_NO_FIRMWARE;
		}
		else if (result == 0xC0) //AMS正常。处于APP0模式
		{
			status = DEVICE_READY;
		}
	}
	else
	{
		status = DEVICE_ERROR;
	}
	return status;
}

static void tmf8801_rst(void)
{
	/* 复位芯片 */
	Device_Write(VHW_PIN_AMS_POWER, 0, 0, 1);
	Device_Write(VHW_PIN_AMS_EN, 0, 0, 0);
	Device_DelayUs(990);
	Device_DelayUs(990);
	Device_DelayUs(990);
	Device_DelayUs(990);
	Device_DelayUs(990);
	Device_Write(VHW_PIN_AMS_POWER, 0, 0, 0);
	Device_Write(VHW_PIN_AMS_EN, 0, 0, 1);
}

static void tmf8801_firmware_init(void)
{
	/* 芯片固件下载 */
	int32_t rc = 0;
	rc = tof8801_wait_for_cpu_ready(&tmf8801); //等待ready
	if (rc)
	{
		__DSENSOR_LOG("ams not ready\n");
	}
	tmf8801_firmware_download(); //下载固件
	device_status = tmf8801_checkWorkState();
	if (device_status == DEVICE_READY)
	{
		tof8801_app0_init_app(&tmf8801);
		__DSENSOR_LOG("ams version: %u.%u.%u.%u\n",
					tmf8801.app0_app.version[0], tmf8801.app0_app.version[1], tmf8801.app0_app.version[2],
					tmf8801.app0_app.version[3] | (tmf8801.app0_app.version[4] << 8));
	}
}

/**
  * @brief  读取校准数据
  * @note   
  *         
  * @return SUCCESS/ERROR
  */
static ErrorStatus tmf8801_read_calibration_data(uint8_t *data)
{
	if (tof8801_app0_process_irq(&tmf8801) == 0)
	{
		if (tof8801_app0_get_fac_calib(&tmf8801, data, APP0_FAC_CALIB_SIZE) == 0)
		{
			return SUCCESS;
		}
	}
	return ERROR;
}

/**
  * @brief  读距离
  * @note   
  *         
  * @return SUCCESS/ERROR
  */
static ErrorStatus tmf8801_ps_read(uint16_t *ps, uint8_t *cfi)
{
	__NVRAM static uint8_t last_sn = 0;
	__NVRAM static uint16_t last_ps = 0;
	uint8_t sn = 0;

	if (tof8801_read_distance(&tmf8801) == 0)
	{
		sn = (uint8_t)tof8801_app0_get_last_result(&tmf8801, ps, cfi);
		if (sn != last_sn)
		{
			__DSENSOR_DEBUG_LOG("%d\t%d\t%d\r\n", *ps, *cfi, abs(last_ps - *ps));
			last_sn = sn;
			last_ps = *ps;
			return SUCCESS;
		}
	}
	else
	{
		device_status = tmf8801_checkWorkState();
		if(device_status != DEVICE_READY)
		{
			DSENSOR_LOG("read err, status: %d\r\n", device_status);
		}
	}
	return ERROR;
}

/**
  * @brief  启动捕获
  * @note         
  */
static void tmf8801_start_capture(void)
{
	/* 传感器关闭 */
	if (sensor_swi == DISABLE)
	{
		return;
	}

	/* 异常处理 */
	dsensor_err_cnt++;
	if(dsensor_err_cnt > 3)
	{
		DSENSOR_LOG("ams err, reset\r\n");
		dsensor_err_cnt = 0;
		tmf8801_rst();
		device_status = DEVICE_ERROR;
	}
    else if (dsensor_err_cnt > 1)
    {
        //上次启动还没处理中断，直接返回
        return;
    }

	/* 处理传感器故障 */
	if (device_status != DEVICE_READY)
	{
		device_status = tmf8801_checkWorkState();
		if (device_status == DEVICE_ERROR) //硬件错误或无固件
		{
			DSENSOR_LOG("ams err, reinit\r\n");
			tmf8801_rst();
		}
		else if (device_status == DEVICE_NO_FIRMWARE)
		{
			DSENSOR_LOG("no firmware, downloading...\r\n");
			tmf8801_firmware_init();
		}

		if (device_status != DEVICE_READY)
        {
			return;
        }
	}

    tmf8801.app0_app.cal_flags = 0;
	tof8801_app0_set_fac_calib(&tmf8801, cal_data, 14);
	tof8801_app0_set_my_capture_settings(&tmf8801, APP0_PERIOD_SINGEL);
	int32_t rc = tof8801_app0_start_capture(&tmf8801);
	if (rc)
	{
		device_status = DEVICE_ERROR;
		DSENSOR_LOG("init tmf8801 failed");
	}
}

/**
  * @brief  校准
  * @note         
  */
static ErrorStatus tmf8801_calibration(void)
{
	/* 复位校准值 */
	Dsensor_Cfg_stu_t cfg;
	memset(&cfg, 0, sizeof(Dsensor_Cfg_stu_t));
	OSAL_NvWrite(OSAL_OFFSET(Dsensor_Cfg_stu_t, cfi), &cfg, sizeof(Dsensor_Cfg_stu_t));
	memcpy(cal_data, default_cal_data, sizeof(cal_data));

	/* 重启传感器 */
	tmf8801_rst();
	tmf8801_firmware_init();
	tmf8801_start_capture();

	int32_t ret = tof8801_app0_stop_capture(&tmf8801);
	if (ret == 0)
	{
		ret = tof8801_factory_calibration(&tmf8801);
	}
	tofCalFlag = SET;
	return ((ret == 0) ? SUCCESS : ERROR);
}

/**
  * @brief  传感器初始化
  * @note   
  */
static void tmf8801_init(void)
{
	static uint8_t init_flag = 0;
	/* 注册中断回调 */
	if (init_flag == 0)
	{
		init_flag = 1;

		InputPort_stu_t port[] = 
		{
			{"DSENSOR_INT", VHW_IRQ_AMS, INPUT_PORT_LOGIC_LOW, INPUT_PORT_FUNC_SINGLE}, //中断脚
        };
        /* 注册端口 */
        InputPort_Registered(port, OSAL_LENGTH(port), dsensor_irqHandle);
	}
}

// static void dsensor_update_baseline_value(uint16_t ps, uint32_t interval)
// {
// 	__NVRAM static uint16_t first_value = 0;
// 	__NVRAM static uint32_t stable_time = 0;

// 	uint32_t stable_threshold = (first_value >= (baseline_value - 50)) ? 1000 : 1000 * 10;

// 	/* 更新基准值 */
// 	if (abs(first_value - ps) < 50)
// 	{
// 		stable_time += interval;
// 	}
// 	else
// 	{
// 		first_value = ps;
// 		stable_time = 0;
// 	}

// 	if (stable_time >= stable_threshold)
// 	{
// 		baseline_value = ps;
// 	}
// 	__DSENSOR_DEBUG_LOG("time : %d, first_value : %d, value : %d\r\n", stable_time, first_value, baseline_value);
// }

static uint8_t dsensor_get_cfi(void)
{
	uint8_t cfi = 0xff;
	if(sensor_sensitivity == DSENSOR_SENS_H)
	{
		cfi = 30;
	}
	else if(sensor_sensitivity == DSENSOR_SENS_M)
	{
		cfi = 40;
	}
	else if(sensor_sensitivity == DSENSOR_SENS_L)
	{
		cfi = 50;
	}
	return cfi;
}

static uint16_t dsensor_get_max_distance(void)
{
	uint16_t ps = MAX_WAKE_DISTANCE;
	if (sensor_type == DSENSOR_ND)
	{
		if(sensor_sensitivity == DSENSOR_SENS_H)
		{
			ps = 1000;
		}
		else if(sensor_sensitivity == DSENSOR_SENS_M)
		{
			ps = 800;
		}
		else if(sensor_sensitivity == DSENSOR_SENS_L)
		{
			ps = 600;
		}
	}
	return ps;
}

/**
  * @brief  启动捕获
  * @note         
  */
static void dsensor_start_capture(void)
{
	/* 传感器关闭 */
	if (sensor_swi == DISABLE)
	{
		return;
	}
	if (sensor_type == DSENSOR_AMS)
	{
		tmf8801_start_capture();
	}
	else if (sensor_type == DSENSOR_ND)
	{
		nd03b_start_capture();
	}
}

/**
  * @brief  读距离
  * @note   
  *         
  * @return SUCCESS/ERROR
  */
static ErrorStatus dsensor_ps_read(uint16_t *ps, uint8_t *cfi)
{
	ErrorStatus ret = ERROR;
	if (sensor_type == DSENSOR_AMS)
	{
		ret = tmf8801_ps_read(ps, cfi);
	}
	else if (sensor_type == DSENSOR_ND)
	{
		ret = nd03b_ps_read(ps, cfi);
	}
	return ret;
}

/**
  * @brief  处理距离数据
  * @param  mode(0：正常状态 / 1：休眠状态)
  * @note   
  */
static ErrorStatus dsensor_process(uint8_t mode)
{
	ErrorStatus ret = ERROR;
	uint16_t ps;
	uint8_t cfi;
	__NVRAM static uint32_t valid_times = 0;
	__NVRAM static uint32_t baseline_value = 0xffff;

	/* 清除传感器异常计数 */
	dsensor_err_cnt = 0;

	if (dsensor_ps_read(&ps, &cfi) == ERROR)
	{
		return ERROR;
	}

	if (baseline_value == 0xffff) //首次上电
	{
		baseline_value = ps;
		return ERROR;
	}

	dsensor_msg.type = READ_DISTANCE;
	dsensor_msg.ps_msg.distance = ps;
	dsensor_msg.ps_msg.cfi = cfi;

	if(demo_mode)
	{
		dsensor_msg.ps_msg.action = USER_ACTION_INVALID;
		OSAL_MessagePublish(&dsensor_msg, sizeof(DsensorMsg_t));
		return ERROR;
	}

	if (sensor_type == DSENSOR_ND) //nd03传感器只检测一次
	{
		uint16_t max_ps = dsensor_get_max_distance();
		__DSENSOR_DEBUG_LOG("%d\t%d\r\n", ps, cfi);
		if (ps >= 100 && ps <= max_ps)
		{
			dsensor_msg.ps_msg.action = USER_ACTION_PRESS;
			if(!mode)
			{
				OSAL_MessagePublish(&dsensor_msg, sizeof(DsensorMsg_t));
			}
			else
			{
				dsensor_wake_flag = SET;
			}
			return SUCCESS;
		}
		return ERROR;
	}

	if (ps > (MAX_WAKE_DISTANCE + VALID_THRESHOLD) || ps == 0)
	{
		ps = MAX_WAKE_DISTANCE + VALID_THRESHOLD;
	}

	// dsensor_update_baseline_value(ps, measure_interval);

	// __DSENSOR_DEBUG_LOG("%d\t%d\t%d\r\n", baseline_value, ps, valid_times);

	if ((abs(baseline_value - ps) > VALID_THRESHOLD) && (cfi > dsensor_get_cfi())) //有距离变化
	{
		valid_times++;
		if (valid_times == 1) //再检测一次，看是否是误唤醒
		{
			Device_Enable(VHW_IIC_AMS);
			dsensor_start_capture();
			Device_Disable(VHW_IIC_AMS);
		}
		else if (valid_times >= 2) //检测到两次就认为有人触发
		{
			valid_times = 0;
			__DSENSOR_LOG("body in\r\n");
			/* send msg */
			dsensor_msg.ps_msg.action = USER_ACTION_PRESS;
			if(!mode)
			{
				OSAL_MessagePublish(&dsensor_msg, sizeof(DsensorMsg_t));
			}
			else
			{
				dsensor_wake_flag = SET;
			}
			ret = SUCCESS;
		}
	}
	else
	{
		valid_times = 0;
	}
	if (valid_times == 0) //无人时更新比较值
	{
		baseline_value = ps;
	}
	return ret;
}


/**
  * @brief  处理校准数据
  * @note   
  */
static void dsensor_process_calibration(void)
{
    uint8_t data[14] = {0};

    dsensor_msg.type = CAL_RESULT;

    if (tmf8801_read_calibration_data(data) == SUCCESS)
    {
        OSAL_NvWrite(OSAL_OFFSET(Dsensor_Cfg_stu_t, cal_data), data, sizeof(data));
        dsensor_msg.cal_msg.result = 1;
    }
    else
    {
        dsensor_msg.cal_msg.result = 0; 
    }
	__DSENSOR_LOG("dsensor calibration %d\r\n", dsensor_msg.cal_msg.result);
    /* 通知应用层校准结果 */
    OSAL_MessagePublish(&dsensor_msg, sizeof(DsensorMsg_t));
}

/**
  * @brief  传感器中断处理
  * @note   只创建中断事件，任务里面处理
  * @return SUCCESS/ERROR
  */
static void dsensor_irqHandle(char *name, uint8_t status, uint8_t times)
{
    if (status == INPUT_PORT_STA_PRESS)
    {
	    OSAL_EventCreateFromISR(COMP_DSENSOR);
    }
}

/**
  * @brief  传感器初始化
  * @note   
  */
static void dsensor_init(void)
{
    sensor_type = dsensor_find_sensor_type();
    if (sensor_type == DSENSOR_AMS)
    {
        tmf8801_init();
    }
    else if (sensor_type == DSENSOR_ND)
    {
        nd03b_init();
    }
	/* 加载初始值 */
	Dsensor_Cfg_stu_t cfg;
	OSAL_NvRead(0, &cfg, sizeof(Dsensor_Cfg_stu_t));
	if (_isDataValid(cfg.cal_data, sizeof(cfg.cal_data)) == SUCCESS)
	{
		memcpy(cal_data, cfg.cal_data, sizeof(cfg.cal_data));
	}
	if (cfg.cfi >= DSENSOR_SENS_H && cfg.cfi <= DSENSOR_SENS_OFF)
	{
		sensor_sensitivity = cfg.cfi;
	}
	else
	{
		sensor_sensitivity = DSENSOR_SENS_H;
	}
	err_process((sensor_type == DSENSOR_UNKNOWN) ? 1 : 0);
}

/**
  * @brief  传感器校准
  * @note   启动校准，校准完成后中断触发读取校准数据
  * @return SUCCESS/ERROR
  */
static ErrorStatus dsensor_calibration(void)
{
	ErrorStatus ret = ERROR;
	if (sensor_type == DSENSOR_AMS)
	{
		ret = tmf8801_calibration();
	}
	else if (sensor_type == DSENSOR_ND) //兼容ams的产测，这里直接返回成功
	{
		Device_DelayMs(100); //发的太快装备反应不过来，加个延时
		ret = SUCCESS;
		dsensor_msg.type = CAL_RESULT;
		dsensor_msg.cal_msg.result = ((ret == SUCCESS) ? 1 : 0); 
		/* 通知应用层校准结果 */
		OSAL_MessagePublish(&dsensor_msg, sizeof(DsensorMsg_t));
	}
	return ret;
}

/**
  * @brief  传感器校准
  * @note   启动校准，校准完成后中断触发读取校准数据
  * @return SUCCESS/ERROR
  */
static ErrorStatus dsensor_calibration_ex(uint8_t mode)
{
	ErrorStatus ret = ERROR;
	if (sensor_type == DSENSOR_ND)
	{
		if (mode == 0x01)
		{
			ret = ((ND03B_Calibration_Offset() == 0x00) ? SUCCESS : ERROR);
		}
		else if (mode == 0x02)
		{
			ret = ((ND03B_Calibration_XTalk() == 0x00) ? SUCCESS : ERROR);
		}
	}

	dsensor_msg.type = CAL_RESULT;
    dsensor_msg.cal_msg.result = ((ret == SUCCESS) ? 1 : 0); 
	__DSENSOR_LOG("dsensor calibration %d\r\n", dsensor_msg.cal_msg.result);
    /* 通知应用层校准结果 */
    OSAL_MessagePublish(&dsensor_msg, sizeof(DsensorMsg_t));
	return ret;
}

/**
  * @brief  传感器使能控制
  * @note
  * @return SUCCESS/ERROR
  */
static ErrorStatus dsensor_power_ctrl(FunctionalState ctrl)
{
	__DSENSOR_LOG("dsensor_power_ctrl(%d)\r\n", ctrl);
	if (ctrl == DISABLE)
	{
		sensor_swi = ctrl;
		if (device_status == DEVICE_READY && sensor_type == DSENSOR_AMS)
		{
			tof8801_app0_stop_capture(&tmf8801);
		}
		Device_Write(VHW_PIN_AMS_EN, 0, 0, 0);
		device_status = DEVICE_POWER_OFF;
	}
    else
    {
		if(sensor_sensitivity == DSENSOR_SENS_OFF)
		{
			__DSENSOR_LOG("dsensor power on fail\r\n");
		}
		else
		{
			sensor_swi = ctrl;
		}
    }
	return SUCCESS;
}

/**
  * @brief  设置传感器灵敏度
  * @note
  * @return SUCCESS/ERROR
  */
static ErrorStatus dsensor_set_sensitivity(Dsensor_Sensitivity_enum_t val)
{
	__DSENSOR_LOG("dsensor_set_sensitivity(%d)\r\n", val);
	if(val >= DSENSOR_SENS_H && val <= DSENSOR_SENS_OFF)
	{
		sensor_sensitivity = val;
		return OSAL_NvWrite(OSAL_OFFSET(Dsensor_Cfg_stu_t, cfi), &sensor_sensitivity, sizeof(sensor_sensitivity));
	}
	return ERROR;
}

/**
  * @brief  获取传感器灵敏度
  * @note
  * @return SUCCESS/ERROR
  */
static Dsensor_Sensitivity_enum_t dsensor_get_sensitivity(void)
{
	__DSENSOR_LOG("dsensor_get_sensitivity(%d)\r\n", sensor_sensitivity);
	return sensor_sensitivity;
}

/**
  * @brief  传感器进入测试模式
  * @note
  * @return SUCCESS/ERROR
  */
static ErrorStatus dsensor_set_demo_mode(uint8_t mode)
{
	demo_mode = mode;
	if (sensor_type == DSENSOR_ND)
	{
		ND03B_SetWorkMode(mode);
	}
	return SUCCESS;
}

/**
  * @brief  唤醒逻辑处理
  * @note
  */
static int32_t Dsensor_WakeupHandle(uint32_t device)
{
	static uint16_t interval = 0;
    if (device == VHW_IRQ_AMS)   //中断唤醒处理
    {
        Device_Enable(VHW_IIC_AMS);
        int32_t ret = dsensor_process(1);
        Device_Disable(VHW_IIC_AMS);
        return ret;
    }
    else if (device == vTIMER_1) //定时唤醒处理
    {
		if (KOS_PARAM_WAKE_INTERVAL < 500)
		{
			interval += KOS_PARAM_WAKE_INTERVAL;
			if (interval < 500)
			{
				return 0;
			}
			measure_interval = interval;
			interval = 0;
		}
        Device_Enable(VHW_IIC_AMS);
        dsensor_start_capture();
        Device_Disable(VHW_IIC_AMS);
        return 0;
    }
    else
    {
        return -1;
    }
}
COMPONENT_WAKEUP_EXPORT(COMP_DSENSOR, Dsensor_WakeupHandle, VHW_IRQ_AMS);
#ifndef AMS_CLOSE_SEELP_SCAN
COMPONENT_WAKEUP_EXPORT(COMP_DSENSOR, Dsensor_WakeupHandle, vTIMER_1);
#endif

/**
  * @brief  距离传感器任务函数
  *
  * @param  event：当前任务的所有事件
  *
  * @return 返回未处理的事件
  */
static uint32_t Dsensor_Task(uint32_t event)
{
	/* 系统启动事件 */
	if (event & EVENT_SYS_START)
	{
		dsensor_init();
        InputPort_EnableProt("DSENSOR_INT");

		measure_interval = 500;
		if (sensor_type != DSENSOR_UNKNOWN)
		{
			OSAL_EventRepeatCreate(COMP_DSENSOR, EVENT_READ_DISTANCE, measure_interval, EVT_PRIORITY_LOW);
		}

		if(dsensor_wake_flag != RESET)
		{
			dsensor_wake_flag = RESET;
			OSAL_MessagePublish(&dsensor_msg, sizeof(DsensorMsg_t));
		}

		SYS_API(dsensor_calibration);
		SYS_API(dsensor_calibration_ex);
		SYS_API(dsensor_power_ctrl);
		SYS_API(dsensor_set_sensitivity);
		SYS_API(dsensor_get_sensitivity);
		SYS_API(dsensor_set_demo_mode);

		return (event ^ EVENT_SYS_START);
	}

	/* 系统休眠事件 */
	if (event & EVENT_SYS_SLEEP)
	{
		measure_interval = KOS_PARAM_WAKE_INTERVAL;
        InputPort_DisableProt("DSENSOR_INT");
		Device_Disable(VHW_IIC_AMS);
		return (event ^ EVENT_SYS_SLEEP);
	}

	/* 扫描tof */
	if (event & EVENT_READ_DISTANCE)
	{
        if (OSAL_GetAutoSleepTime() > 500)
        {
		    dsensor_start_capture();
			if (device_status == DEVICE_READY)
			{
				err_process(0);
			}
			else if (device_status == DEVICE_ERROR)
			{
				err_process(1);
			}
        }
		return (event ^ EVENT_READ_DISTANCE);
	}

	if (event & EVENT_SYS_ISR)
	{
        if (tofCalFlag == SET)
		{
            //校准中断
            tofCalFlag = RESET;
            dsensor_process_calibration();
		}
		else
		{
            //读距离中断
			dsensor_process(0);
		}
		return (event ^ EVENT_SYS_ISR);
	}

	return 0;
}
COMPONENT_TASK_EXPORT(COMP_DSENSOR, Dsensor_Task, sizeof(Dsensor_Cfg_stu_t));
